/*
 * Copyright (c) 2006-2020, RT-Thread Development Team
 *
 * SPDX-License-Identifier: Apache-2.0
 *
 * Change Logs:
 * Date           Author       Notes
 * 2021-03-09     CGY       the first version
 */
#include "head_user.h"

RC_HandleTypedef RC_Handle;

static void RC_Mode(uint8_t ms);
static void RC_to_motor(uint8_t ms);

/* 遥控器数据处理 */
void RC_Task(void *parameter)
{
    static uint32_t tick;
    while(1)
    {
        RC_Handle.rcUpdate(10);

        if(tick%2 == 0)
        {
            RC_to_motor(20);
        }

        tick++;
        rt_thread_mdelay(10);    //调度
    }
}


/* 遥控数据单位化和去除死区 */
static float data_to_unit_dead(int16_t rawData, int16_t deadband)
{
    float norm = ((float)data_to_deadzone(rawData,0,deadband) / (float)(500-deadband));
    return norm;
}
/* 遥控数据转换 */
static float rcSwitch(float unit, float max)
{
    return unit * max;
}


/**********************************************************************
* 名    称：RC_update
* 说    明：遥控器原始数据进行规范化，加死区，单位化
**********************************************************************/
static void RC_update(uint8_t ms)
{
    /*-----------------------------------------------------------------------------------------------------------------------*/
    /* 遥控器通道数据更新 */
    for(uint8_t i=0; i<CH_NUM; i++)
    {
        if(RC_Handle.rc_raw[i] != 0)                                                 //如果该通道有值,处理通道数据
        {
            RC_Handle.rc_norm[i] = 0.72f *(RC_Handle.rc_raw[i] - 990);               //处理成+-500摇杆量,1.21是为遥控器做的适配
            RC_Handle.rc_norm[i] = (int16_t)limit(RC_Handle.rc_norm[i],-500,500);    //限制到+—500
            RC_Handle.rc_ori[ i] = (RC_Handle.rc_norm[i] + 1500);
        }
        /*这里要改一下,这个样子不能设置失控保护*/
        else
        {
            RC_Handle.rc_norm[i] = 0;                                                //没值全部默认设为中位(1500)//遥控器不开相当于遥控器输入0
        }

        if(RC_Handle.rc_norm[i] < (-200))            //处理通道 档位
        {
            RC_Handle.ch_state[i] = LO;
        }
        else if(RC_Handle.rc_norm[i] > (200))
        {
            RC_Handle.ch_state[i] = HI;              //0-300:低档  300-700：中档
        }
        else
        {
            RC_Handle.ch_state[i] = CE;              //700-1000:高档
        }
    }

    /*遥控数据单位化和加死区,最后重新回归正常数据*/
    RC_Handle.rc_uint[ROL] = data_to_unit_dead(RC_Handle.rc_norm[ROL], 40);
    RC_Handle.rc_uint[PIT] = data_to_unit_dead(RC_Handle.rc_norm[PIT], 40);
    RC_Handle.rc_uint[YAW] = data_to_unit_dead(RC_Handle.rc_norm[YAW], 80);
    RC_Handle.rc_uint[THR] = data_to_unit_dead(RC_Handle.rc_norm[THR], 80);

    RC_Handle.rc_dat[ROL]  = (int16_t)(RC_Handle.rc_uint[ROL]*500);
    RC_Handle.rc_dat[PIT]  = (int16_t)(RC_Handle.rc_uint[PIT]*500);
    RC_Handle.rc_dat[YAW]  = (int16_t)(RC_Handle.rc_uint[YAW]*500);
    RC_Handle.rc_dat[THR]  = (int16_t)(RC_Handle.rc_uint[THR]*500);

    RC_Handle.rc_dat[THR]  = (int16_t)constrainf(RC_Handle.rc_dat[THR], -500, 400);    //油门限制到-400到400 (不失真限制)
    /*-----------------------------------------------------------------------------------------------------------------------*/

    /* 遥控指令设置, 设置飞行模式, 解锁/上锁 */
    RC_Mode(ms);
}



/**********************************************************************
* 名    称：RC_Modeset
* 说    明：这个函数仅仅把飞机状态标志位做处理   飞机依靠这些标志位运行相应的控制
* 包含有任务模式flag置位，复位
**********************************************************************/
static void RC_Modeset(Flifht_state_show state)
{
    /* 清空标志位 */
    RC_Handle.Flyflag.AllMarks &= ~0xFFF;    //清除前12位

    /* 根据飞行模式配置标志位 */
    switch (state)
    {
        case Stabilize:                                     //自稳模式
        {
            RC_Handle.Flyflag.Stabilize     = ON;
            RC_Handle.Flyflag.Height_ctrl   = acce_mode;    //仅自稳模式时用油门加速度控制,其它时候均用速度控制
        }break;
        case Fly_Altitude:                              //定高模式包含自稳模块
        {
            RC_Handle.Flyflag.Stabilize = ON;
            RC_Handle.Flyflag.Altitude  = ON;
            RC_Handle.Flyflag.Height_ctrl   = sped_mode;
        }break;
        case Position:                              //定点模式包含定高自稳模块
        {
            RC_Handle.Flyflag.Stabilize = ON;
            RC_Handle.Flyflag.Altitude  = ON;
            RC_Handle.Flyflag.Position  = ON;
            RC_Handle.Flyflag.Height_ctrl   = sped_mode;
        }break;
        case Headless:                              //无头模式运行在定点模式下
        {
            RC_Handle.Flyflag.Stabilize = ON;
            RC_Handle.Flyflag.Altitude  = ON;
            RC_Handle.Flyflag.Position  = ON;
            RC_Handle.Flyflag.Headless  = ON;
            RC_Handle.Flyflag.Height_ctrl   = sped_mode;
        }break;
        case One_key_fly:
        {
            RC_Handle.Flyflag.Stabilize = ON;
            RC_Handle.Flyflag.Altitude  = ON;
            RC_Handle.Flyflag.Position  = ON;
            RC_Handle.Flyflag.Auto      = ON;
            RC_Handle.Flyflag.Height_ctrl   = sped_mode;
        }break;
        case Beeper:                                //寻机\紧急停机模式,机载喇叭开启关闭飞机输出
        {
            RC_Handle.Flyflag.Finder    = ON;
        }break;
        default:break;
    }
}


/**********************************************************************
* 名    称：rc_unlock
* 功    能：实现 内外八 均能解锁上锁
* 说    明：本函数仅实现解锁上锁标志位置位清零，对外除解锁无影响
**********************************************************************/
static void rc_unlock(uint8_t ms)
{
    static uint16_t Unlock_count, Unlock_state;

    switch( Unlock_state )
    {
        case 0:
        {
            if((RC_Handle.ch_state[THR]==LO&&RC_Handle.ch_state[YAW]==HI&&
                RC_Handle.ch_state[ROL]==LO&&RC_Handle.ch_state[PIT]==HI)||
               (RC_Handle.ch_state[THR]==LO&&RC_Handle.ch_state[ROL]==HI&&
                RC_Handle.ch_state[YAW]==LO&&RC_Handle.ch_state[PIT]==HI))
            {
                if(RC_Handle.Flyflag.BodyState == unlock)   //解锁->上锁立即切换
                {
                    RC_Handle.Flyflag.BodyState = lock;
                }
                Unlock_count += ms;                             //解锁计时
            }
            else
            {
                Unlock_count = 0;                               //持续计时,间断清零
            }
            if( Unlock_count >= 1000  )                         //2s
            {
                Unlock_count = 0;
                RC_Handle.Flyflag.BodyState = waitunlock;       //飞机首次解锁
                Unlock_state = 1;                               //解锁上锁流程控制
            }
        }break;
        case 1:                                                 //遥控归中
        {
            if(RC_Handle.ch_state[THR]==CE&&RC_Handle.ch_state[YAW]==CE&&
               RC_Handle.ch_state[PIT]==CE&&RC_Handle.ch_state[ROL]==CE)
            {
                Unlock_state = 0;
                RC_Handle.Flyflag.BodyState = unlock;       //摇杆归中才清零首次解锁标志位(为了防止回中过程中进入运行态)
            }
        }break;
        default:break;
    }
}

/**********************************************************************
* 名    称：RC_Mode
* 说    明：此函数仅处理RC数据，传出控制人的目的。仅对相关标志位进行操作
*       至于飞机执不执行，交由其它函数判断处理.
*       自主飞行模式下，飞机解锁上锁标志位可随便用，具体原理函数中有详细说明
**********************************************************************/
static void RC_Mode(uint8_t ms)
{
    /* 设置飞行器飞行模式  */
    if(RC_Handle.ch_state[AUX3]==HI)
    {
        RC_Modeset( One_key_fly );  //一键起飞模式
    }
    else if(RC_Handle.ch_state[AUX1]==HI)
    {
        RC_Modeset( Position );     //定点模式
    }
    else if(RC_Handle.ch_state[AUX1]==CE)
    {
        RC_Modeset( Fly_Altitude );     //定高模式
    }
    else if(RC_Handle.ch_state[AUX1]==LO)
    {
        RC_Modeset( Stabilize );    //自稳模式
    }

    if(RC_Handle.ch_state[AUX2]==HI)
    {
        RC_Modeset( Beeper );       //寻机/紧急停机模式
    }

    /* 模式附加  */
    if(RC_Handle.ch_state[AUX4]==HI)
    {
        RC_Handle.Flyflag.Fence      = ON;
    }

    /* 飞机解锁判断 */
    rc_unlock(ms);

    /*===================================================================*/
    /* 电调校准,先判断遥控器类型 */
    /*===================================================================*/
}


/**********************************************************************
* 名    称：RC_to_motor
* 说    明：完成对任务组件的基本支持
**********************************************************************/
static void RC_to_motor(uint8_t ms)
{
    static uint16_t unrun_count, run_state;

    switch ( RC_Handle.Flyflag.BodyState )
    //switch ( ESC_Cali )
    {
        case lock:      /* 飞机上锁时的一系列处理,此部分兼容遥控/任务 */
        {
            MT_Handle.stateUpdate( Moter_lock );
            run_state = 0;                                              //复位本函数标志位
            LPF1_Reset(&LPF_1[LPF_motor_hover], motor_hover, 0.02);     //motor_hover 悬停油门
        }break;
        case waitunlock:/* 此判断仅说明遥控解锁后尚未回中, 任务控制不会置位这个标志位, 所以这里的限制对任务无效 */
        {
            MT_Handle.stateUpdate( Moter_lazy );  //解锁怠速,不进行姿态控制
        }break;
        case unlock:    /* 飞机解锁,正式进入飞机控制 */
        {
#ifndef AC_USING_AUTOTASK
            /*-----------------------------------------------------------------------------------------------------------------------*/
            if( (RC_Handle.rc_dat[THR] > 10 || run_state == 1) )
            {
                /* 电机进入运行态,各个算法控制开始起作用 */
                MT_Handle.stateUpdate( Moter_obey );

                /* 运行状态标志位置1,保证遥控器闲置也能持续运行态 */
                run_state = 1;

                /* 悬停油门快速补偿,用于遥控器高度速度控制 */
                LPF1(10, &LPF_1[LPF_motor_hover]);

                /* 落地自动上锁 : 油门拉低, 且距离地面10厘米时判断为落地 */
                if( (RC_Handle.rc_dat[THR]<-300 && absolute(BODY.nfSpeed.z)<10) )
                {
                    unrun_count += ms;
                }
                else
                {
                    unrun_count = 0;
                }
                if( unrun_count >= 2000 )                            //油门拉到最低，持续2秒，上锁
                {
                    RC_Handle.Flyflag.BodyState = lock;
                    unrun_count = 0;
                }
            }
            else
            {
                MT_Handle.stateUpdate( Moter_lazy );
            }
            /*-----------------------------------------------------------------------------------------------------------------------*/
#else
            static uint16_t task_count, task_flag;

            /*===================================================================*/
            /* 任务模式部分 */
            if( RC_Handle.Flyflag.Auto )
            {
                task_count += ms;
            }
            else
            {
                task_count = 0;
                task_flag  = 0;
            }
            if (task_count >= 1500 )
            {
                task_flag = 1;      //任务延时进入怠速后,开始运行
            }
            /*===================================================================*/

            if( (RC_Handle.rc_dat[THR] > 10 || run_state == 1) || (task_flag == 1) )          //解锁后遥控器有数据,进入运行,运行中靠状态位进入（这样即使遥控器没输出依然能维持在运行态）
            {
                MT_Handle.stateUpdate( Moter_obey );      //电机进入运行态，各个算法控制开始起作用

                run_state = 1;                                      //运行状态标志位置1，只保证遥控器不操作也能进入运行态,遥控，自主切换时飞机始终在运行

                LPF1(10, &LPF_1[LPF_motor_hover]);                  //悬停油门快速补偿，用于遥控器高度速度控制

                /* 降落上锁 ，仅支持遥控组件，任务组件靠高度控制组件返回的flag判断执行上锁 */
                if( (RC_Handle.rc_dat[THR]<-300 && absolute(BODY.nfSpeed.z)<10) )      //落地上锁,特殊上锁操作,兼容任务和遥控器
                {
                    unrun_count += ms;
                }
                else
                {
                    unrun_count = 0;
                }
                if( unrun_count >= 1500 )                            //油门拉到最低，持续2秒，上锁
                {
                    RC_Handle.Flyflag.BodyState = lock;
                    unrun_count = 0;
                }
            }
            else
            {
                MT_Handle.stateUpdate( Moter_lazy );
            }
#endif
        }break;
        case ESC_Cali:
        {
            MT_Handle.stateUpdate( Motor_ESCCali );
        }break;
        default:break;
    }
}


/*-------------------------------------------------------------------------------------------------*/
static int remote_init(void)
{
    RC_Handle.rcUpdate      = RC_update;
    RC_Handle.datswitch     = rcSwitch;

    return 0;
}

INIT_APP_EXPORT(remote_init);
/*-------------------------------------------------------------------------------------------------*/
